
#include "pso.h"

Pso::Pso()
{
   m_bestFitnessParticleIndex = 0;
   m_algorithm = PSO;
   m_finish = false;
}

Pso::Pso(const Pso& pso) : QObject( 0 )
{
   m_maxV    = pso.m_maxV;
   m_maxX    = pso.m_maxX;
   m_maxIter = pso.m_maxIter;
   m_cutoff  = pso.m_cutoff;
   m_weight  = pso.m_weight;
   m_weightType = pso.m_weightType;
      
   m_bestFitness = pso.m_bestFitness;
   m_bestFitnessParticleIndex = pso.m_bestFitnessParticleIndex;
      
   m_swarm = pso.m_swarm;
      
   m_inputSet = pso.m_inputSet;
   m_targetSet = pso.m_targetSet;
   
   m_algorithm = pso.m_algorithm;
}

Pso& Pso::operator=(const Pso& pso)
{
   m_maxV    = pso.m_maxV;
   m_maxX    = pso.m_maxX;
   m_maxIter = pso.m_maxIter;
   m_cutoff  = pso.m_cutoff;
   m_weight  = pso.m_weight;
   m_weightType = pso.m_weightType;
      
   m_bestFitness = pso.m_bestFitness;
   m_bestFitnessParticleIndex = pso.m_bestFitnessParticleIndex;
      
   m_swarm = pso.m_swarm;
      
   m_inputSet = pso.m_inputSet;
   m_targetSet = pso.m_targetSet;
   
   m_algorithm = pso.m_algorithm;
   
   return *this;
}


void Pso::setAlgorithm(int algorithm)
{
   m_algorithm = algorithm;
}


void Pso::addToSwarm( NeuralNetwork ann )
{
   Particle tmp;
   tmp.setNeuralNetwork( ann );
   m_swarm.append( tmp );
}

void Pso::addToSwarm( Particle p )
{
   m_swarm.append( p );
}


void Pso::clearSwarm()
{
   m_swarm.clear();
}


void Pso::clearTrainingSet()
{
   m_inputSet.clear();
   m_targetSet.clear();
}



void Pso::addTrainingSet( const QList< float >& input, const QList< float >& target )
{
   if( m_inputSet.isEmpty() || m_targetSet.isEmpty() ||
      ( m_inputSet.first().size() == input.size() && m_targetSet.first().size() == target.size() ) )
   {
      m_inputSet.append( input );
      m_targetSet.append( target );
   }
}

void Pso::addTrainingSet(const QList< float >& set, int splitPoint)
{
   if( splitPoint >= set.size() || splitPoint <= 0 ) return;
   
   QList< float > input = set.mid( 0, splitPoint );
   QList< float > target = set.mid( splitPoint );
   
   addTrainingSet( input, target );
}

int Pso::trainingSetSize()
{
   return m_inputSet.size();
}



void Pso::optimize()
{
   // HACK backpropagation
   if( m_algorithm == 2 )
   {
      m_finish = false;
      NeuralNetwork &net = m_swarm.first().m_ann;
      m_bestFitness = 999999;
      m_bestFitnessParticleIndex = 0;
      
      emit newBestFitnessFound( m_bestFitness );
      emit iterationCompleted( 0 );
      
      float totalError = fabs(m_cutoff)+1.0f; // init larger
      for( int epoch = 0;epoch<m_maxIter && m_cutoff<fabs(totalError);epoch++ )
      {
	 // browse through example set
	 for( int i = 0; i < m_inputSet.size(); i++ )
	 {
// 	 foreach( QList< float > list, exampleDataset ) // for each example in dataset
// 	    inputVector = list.mid( 0, net.inputSize() );
// 	    
// 	    // target = e's output vector
// 	    QList< float > target = list.mid( net.inputSize() );
	    if( m_finish ) return;
	    emit sampleCompleted( i );
	    if( net.backpropagate( net.output( m_inputSet[i] ), m_targetSet[i] ) != 0 ) continue;
	 }
	 
	 totalError = net.totalError();
	 
	 if( totalError < m_bestFitness )
	 {
	    m_bestFitness = totalError;
	    emit newBestFitnessFound( m_bestFitness );
	 }
	 
	 emit iterationCompleted( epoch + 1 );
      }
      
      return;
   }
   
   
   
   
   m_finish = false;
   bool firstRun = true;
   float weight_up;
   
   int iter = 0;
   do
   {
      qDebug( "iter %d", iter );
      switch( m_weightType )
      {
	 //time variant weight, linear from weight to 0.4
	 case TIME_VARIANT:
	    weight_up = ( m_weight - 0.4 ) * ( m_maxIter - iter ) / m_maxIter + 0.4;
	    break;
		  
	 // constant inertia weight
	 case CONSTANT:
	 default:
	    weight_up = m_weight;
	    break;
      }

      int swarmSize = m_swarm.size();
      
      for( int i = 0;i<swarmSize;i++ )
      {
	 Particle &particle = m_swarm[i]; // foreach particle
	 
	 float fitness = particle.fitness( m_inputSet, m_targetSet );
	 
	 if( iter == 0 || fitness < particle.pbest() ) // minimize error
	 {
	    particle.setPbest( fitness );
	    	    
	    // set pbestx-es
	    int d = particle.dimension();
	    for( int j = 0;j<d;j++ )
	    {
	       particle.setPbestx( particle.x(j), j );
	    }
	 }
	 
	 // update best fitness
	 if( firstRun )
	 {
	    m_bestFitness = fitness;
	    m_bestFitnessParticleIndex = 0;
	    firstRun = false;
	 }
	 
	 if( fitness < m_bestFitness )
	 {
	    m_bestFitness = fitness;
	    m_bestFitnessParticleIndex = i;
	    emit newBestFitnessFound( fitness );
	    if( fitness <= m_cutoff ) return;
	 }
      }
      
      
      for( int i = 0;i<swarmSize;i++ )
      {
	 Particle &particle = m_swarm[i]; // foreach particle
	 
	 int d = particle.dimension();
	 for( int j = 0;j<d;j++ )
	 {
	    // Calculate particle velocity according to the velocity equation (1)
	    float velocity;
	    if( m_algorithm == PSO )
	       velocity = weight_up * particle.v( j ) +
		   2 * (rand()/(float)RAND_MAX) * (particle.pbestx(j) - particle.x(j)) +
		   2 * (rand()/(float)RAND_MAX) * (m_swarm[m_bestFitnessParticleIndex].pbestx(j) - particle.x(j));
	    else
	       velocity = weight_up * particle.v( j ) +
		   2 * (rand()/(float)RAND_MAX) * (particle.pbestx(j) - particle.x(j)) +
		   2 * weight_up * (rand()/(float)RAND_MAX) * (m_swarm[m_bestFitnessParticleIndex].pbestx(j) - particle.x(j)) +
		   2 * weight_up * (rand()/(float)RAND_MAX);
	    if( velocity > m_maxV ) velocity = m_maxV;
	    if( velocity < -m_maxV ) velocity = -m_maxV;
	    particle.setV( velocity, j );
	    
	    // Update particle position according to the position equation (2)
	    float position = particle.x(j) + particle.v(j);
	    if( position > m_maxX ) position = m_maxX;
	    if( position < -m_maxX ) position = -m_maxX;
	    particle.setX( position, j );
	 }
      }
      
      iter++;
      emit iterationCompleted( iter );
   }
   while( m_bestFitness > m_cutoff && iter < m_maxIter && !m_finish );
   // while maximum iterations or minimum error criteria is not attained
}


void Pso::stopOptimizing()
{
   m_finish = true;
}



NeuralNetwork Pso::bestNetwork()
{
   return bestParticle().m_ann;
}

Particle Pso::bestParticle()
{
   Particle particle = m_swarm[m_bestFitnessParticleIndex];
   return particle;
}




void Pso::setMaxV(float v)
{
   m_maxV = v;
}


void Pso::setMaxX(float x)
{
   m_maxX = x;
}

void Pso::setMaxIterations(int iter)
{
   m_maxIter = iter;
}

void Pso::setCutoff(float cutoff)
{
   m_cutoff = cutoff;
}

void Pso::setWeight(float weight)
{
   m_weight = weight;
}

void Pso::setInertiaWeightType(int type)
{
   m_weightType = type;
}
